#ifndef DYN_ODE_SIVS_H
#define DYN_ODE_SIVS_H

#include "dyn_ode_si.h"

/*! Alleviates the code a little bit. */
typedef std::function<double(colvec)> firstIntegral;

/*! \brief Variable stepsize symplectic integrator templated class.
 *
 * We implement a variable step symplectic integrator
 * according to RP's method (Poulin 1997). The class
 * is templated so any fixed step integrator can
 * be used to take a step forward.
 *
 * The order of interpolation of the error function is also a free
 * parameter.
 */

template <class SIFS>
class SIVS
{
public:
    /*! Constructor defines the variable
     * needed to construct a variable step integrator.
     */
    SIVS(HamiltonianSystem& func,
         std::vector<firstIntegral> _constantsMotion,
         colvec _initCond,
         double _end,
         double _initStepsize,
         double _minStepsize=1.0e-6,
         double _maxStepsize=5.0e-1,
         double _safety=0.85,
         double _tol=1.0e-12)
        : integrator(func,_initCond,_end,_initStepsize)
    {
        // Integrator variables.
        integrator.setMinStepsize(_minStepsize);
        integrator.setMaxStepsize(_maxStepsize);
        integrator.setRelTol(_tol);

        // Variable step implementation variables.
        setConstantsMotion(_constantsMotion);
        setSafety(_safety);
        setupStartingValues();
        sizeVec = startingValues.size();
    }

    /*! @name Accessor Functions */
    //@{
    double getSafety(){return safety;}
    std::vector<firstIntegral> getConstantsMotion(){return constantsMotion;}

    void setSafety(double _safety){safety=_safety;}
    void setConstantsMotion(std::vector<firstIntegral> _constantsMotion){constantsMotion=_constantsMotion;}
    //@}

    /*! @name Integrator Functions. */
    //@{
    colvec next(colvec previousStep);
    mat integrate();
    //@}

    /*! @name Bookkeeping Functions */
    //@{
    void setupStartingValues();                     /// Setups first integrals init. values.
    void computeLocalError(colvec newStep);         /// Compute error on first integrals.
    //@}

protected:
    /*! @name Member Variables */
    //@{
    SIFS integrator;                                /// Templated integrator.
    double safety;                                  /// Safety value.
    std::vector<firstIntegral> constantsMotion;     /// First integrals functional evaluation.
    colvec startingValues;                          /// Starting values of the first integrals.
    colvec steps;                                   /// Steps history.
    mat pastErrors;                                 /// Error history.
    unsigned int sizeVec;                           /// Number of constants of motion.
    //@}
};

template <class SIFS>
colvec SIVS<SIFS>::next(colvec previousStep)
{
    // Compute next step with templated integrator.
    colvec newStep = integrator.next(previousStep);

    // Compute the local error

    // Compute the stabilizing tolerance factors

    // See if local error is smaller than the stabilized tolerance.

    // If it is, take the step and compute estimate the next time step.

    // Else, rescale the current step and redo the preceding calculations.

}

template <class SIFS>
void SIVS<SIFS>::setupStartingValues()
{
    // Set the size
    startingValues.set_size(sizeVec);

    // We loop over the vector and assign the values.
    for (unsigned int i;i<sizeVec;i++)
    {
        startingValues(i) = constantsMotion[i](integrator.getInitCond());
    }
}

template <class SIFS>
void SIVS<SIFS>::computeLocalError(colvec newStep)
{
    // Prepare vectors to hold errors and stabilizing tolerance factors.
    colvec errors(sizeVec);
    colvec stabTol(sizeVec);

    for (unsigned int i=0;i<sizeVec;i++)
    {
        // Compute error.
        errors(i) = (constantsMotion[i](newStep)-startingValues(i))/startingValues(i);

        // Compute stabilizing tolerance factor.

    }
}



#endif // DYN_ODE_SIVS_H
